#include "includes.h"
#include "data.h"
#include "motor.h"

extern double servo_error[3][2];
extern double servo_duty;

uint8 motor_time;

extern uint16 cnt[2];
double motor_error[3][2];

uint16 set_duty[2];
double motor_duty[2];

void motor_duty_set(void){
    /* fix speed duty */
    if(ABS(servo_error[0][0])>=SET_BOUND)
        set_duty[0]=set_duty[1]=MOTOR_DUTY_STEERING;
    else
        set_duty[0]=set_duty[1]=MOTOR_DUTY_STRAIGHT;
}

void motor_error_calc(void){
    if(++motor_time==3)
        motor_time=0;

    motor_error[motor_time][0]=0.02087*set_duty[0]-1.932-cnt[0];
    motor_error[motor_time][1]=0.02003*set_duty[1]+1.888-cnt[1];
}

void pi_control(void){
    const double kp=16,ki=4;
    motor_duty[0]+=kp*(motor_error[motor_time][0]-motor_error[IN_RANGE(motor_time-1, 3)][0])+ki*motor_error[motor_time][0];
    motor_duty[1]+=kp*(motor_error[motor_time][1]-motor_error[IN_RANGE(motor_time-1, 3)][1])+ki*motor_error[motor_time][1];
}

void diff_control(void){
    /* electronic differential structure 
     * to be adjusted */
    motor_duty[0]=motor_duty[0]*(1-3*0.38095*tan(PI*0.00208*servo_duty));
    motor_duty[1]=motor_duty[1]*(1+3*0.38095*tan(PI*0.00208*servo_duty));
}

void motor_control(void){
    speed_evaluate();
    motor_duty_set();
    motor_error_calc();

    /* if errors beyond the range */
    /*
    if(ABS(motor_error[motor_time][0])>=E_BANG&&ABS(motor_error[motor_time][1])>=E_BANG){
        if((cnt[0]>=C_BANG&&cnt[1]>=C_BANG)&&(motor_error[motor_time][0]>0&&motor_error[motor_time][1]>0)){
            motor_duty[0]=motor_duty[1]=REV_DUTY;
            return;
        }

        if((cnt[0]<=S_BANG&&cnt[1]<=S_BANG)&&(motor_error[motor_time][0]<0&&motor_error[motor_time][1]<0)){
            motor_duty[0]=motor_duty[1]=SPD_DUTY;
            return;
        }
    }
    */

    pi_control();

    /*
    diff_control();
    */
}
